#pragma once
#include "cybertron/sensor/noise/NoiseMaker.hpp"
#include "cybertron/sensor/noise/CompositeNoise.hpp"
#include "cybertron/sensor/noise/WhiteNoise.hpp"
#include "cybertron/sensor/util/SensorConfigUtil.hpp"

CYBERTRON_BEGIN

class GPSNoiseMaker : public NoiseMaker
{
public:
	// default noise data from NovAtel IMU-IGM-A1
	GPSNoiseMaker(float horizontalError = 1.0, float verticalError = 0.6, float velocityError = 0.02, float attitudeError = 0.0026/* in rad*/);
	GPSNoiseMaker(const SensorGpsImuConfig& sensorConfig);
	virtual ~GPSNoiseMaker();

	void update(const vec3& pos, const vec3& rotation, uint64_t frame = 0) override;
	inline float horizontalError() const { return m_horizontalError; }
	inline float verticalError() const { return m_verticalError; }
	inline float velocityError() const { return m_velocityError; }
	inline float attitudeError() const { return m_attitudeError; }
	inline const SensorGpsImuConfig& sensorConfig() const { return m_sensorConfig; }
	inline vec3 positionNoise() const { return m_positionNoise.value3d(); }
	inline vec3 attitudeNoise() const { return m_attitudeNoise.value3d(); }
	inline vec3 velocityNoise() const { return m_velocityNoise.value3d(); }

protected:
	float				m_horizontalError;
	float				m_verticalError;
	float				m_velocityError;
	float				m_attitudeError;
	CompositeNoise		m_positionNoise;
	WhiteNoise			m_attitudeNoise;
	WhiteNoise			m_velocityNoise;
	SensorGpsImuConfig	m_sensorConfig;
};

typedef std::shared_ptr<GPSNoiseMaker> GPSNoiseMakerPtr;

CYBERTRON_END